﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.MotionPlanning.Primitives;
using Drifting.MotionPlanning.Systems;
using Drifting.MotionPlanning.PointSets;
namespace Drifting.MotionPlanning.SingleThreading.Algorithms
{
    public class GraphBasedPlanningAlgorithm : TreeBasedPlanningAlgorithm
    {
        public GraphBasedPlanningAlgorithm(BaseSystem system, Random rand)
            : base(system, rand)
        {
        }

        override public void SetRoot(double[] q_root)
        {
            x_root = SpawnState(q_root);
            vertices.AddState(x_root);
        }

        override protected State SpawnState()
        {
            double[] q = null;
            system.SampleState(rand, ref q);
            State x = new GraphState(q, AliasOf(q));

            return x;
        }

        override protected State SpawnState(double[] q)
        {
            if (q == null)
                throw new ArgumentException();
            State x = new GraphState(q, AliasOf(q));

            return x;
        }

        override protected State SpawnState(double[] q, double[] a)
        {
            if (q == null)
                throw new ArgumentException();
            if (a == null)
                throw new ArgumentException();
            if (q.Length != system.DOF_State)
                throw new ArgumentException();
            if (a.Length != system.DOF_Alias)
                throw new ArgumentException();

            State x = new GraphState(q, a);

            return x;
        }

        override public void AliasOf(double[] q, ref double[] alias)
        {
            if (alias == null)
                alias = new double[system.DOF_Alias];
            for (int i = 0; i < system.DOF_Alias; ++i)
                alias[i] = q[i];
        }

        public override void Execute(object input)
        {
            throw new NotImplementedException();
        }

    }
}
